#include <stdio.h>
#include "platform.h"
#include "xparameters.h"
#include "sleep.h"
#include "zrcar.h"
#define MOVE_FORWARD    1500
#define MOVE_BACK       100
#define DANGEROUS_DIS   100
#define MAX_DIS         1000000
#define MOVE_SLEEP      964800
#define TURN_SLEEP      400000
#define TURN_SPEED      16
float ult_data[3];
int car_x = 0;
int car_y = 0;
int face = 0;
int x[10] = {5,10,15,20,15,30,35,12,28,35};
int y[10] = {5, 5,20, 0,30, 8,42,36,45,35};
void car_stop()
{
    zrcar_wheel_l_set(0);
    zrcar_wheel_r_set(0);
}
void car_forward()
{
    zrcar_wheel_l_set(-16);
    zrcar_wheel_r_set(-16);
}
void car_back()
{
    zrcar_wheel_l_set(16);
    zrcar_wheel_r_set(16);
}
void car_right()
{
    zrcar_wheel_l_set(0);
    zrcar_wheel_r_set(-TURN_SPEED);
}
void car_turn_r()
{
    zrcar_wheel_r_set(TURN_SPEED);
    zrcar_wheel_l_set(-TURN_SPEED);
}
void car_left()
{
    zrcar_wheel_l_set(-TURN_SPEED);
    zrcar_wheel_r_set(0);
}
void car_turn_l()
{
    zrcar_wheel_l_set(TURN_SPEED);
    zrcar_wheel_r_set(-TURN_SPEED);
}
void make_map(int map[][50], int old_map[][50]){
    int j,k;
    int ite_time = 0;
    for(j=0;j<50;j++){
        for(k=0;k<50;k++){
            map[j][k] = 1;
            old_map[j][k] = 1;
        }
    }
    int sum = 0;
//  int i;
    while(1)
    {
        sum = 0;
        if(ite_time == 100){
            for(j=0;j<50;j++){
                for(k=0;k<50;k++){
                    if(old_map[j][k] == map[j][k]){
                        sum ++;
                    }
                    printf("%d",map[j][k]);
                    old_map[j][k] = map[j][k];
                }
                printf("\n");
            }
            ite_time = 0;
        }

        ite_time ++;

        zrcar_ultra_get_all(ult_data);
//      for(i = 1; i < 4; i++){
//          printf("u%d : %f mm ",i , ult_data[i-1]);
//      }
//      printf("\n");

        if(ult_data[0] < DANGEROUS_DIS && ult_data[1] < DANGEROUS_DIS
           && ult_data[2] < DANGEROUS_DIS)
        {
            //left corner && right corner
            //turn left
            face = ( face + 2 ) % 4;
            car_turn_l();
            usleep(2*MOVE_SLEEP);
        }
        else if(ult_data[0] < DANGEROUS_DIS && ult_data[1] < DANGEROUS_DIS)
        {
            //left corner
            //turn right
            face = ( face + 1 ) % 4;
            car_turn_l();
            usleep(MOVE_SLEEP);
        }
        else if(ult_data[1] < DANGEROUS_DIS && ult_data[2] < DANGEROUS_DIS){
            //right corner
            //turn left
            face = ( face - 1 ) % 4;
            car_turn_r();
            usleep(MOVE_SLEEP);
        }
        else if(ult_data[0] < DANGEROUS_DIS){
            //only right
            face = ( face + 1 ) % 4;
            car_turn_l();
            usleep(MOVE_SLEEP);
        }
        else if(ult_data[1] < DANGEROUS_DIS){
            //only front
            car_back();
            face = ( face - 1 ) % 4;
            car_turn_r();
            usleep(MOVE_SLEEP);
        }
        else if(ult_data[2] < DANGEROUS_DIS){
            //only left
            face = ( face - 1 ) % 4;
            car_turn_r();
            usleep(MOVE_SLEEP);
        }
        else if(ult_data[0] > MAX_DIS || ult_data[1] > MAX_DIS
                   || ult_data[2] > MAX_DIS){
            switch (face){
                case 0:
                    car_x -= 2;
                    break;
                case 1:
                    car_y -= 2;
                    break;
                case 2:
                    car_x += 2;
                    break;
                case 3:
                    car_y += 2;
                    break;
            }
            car_back();
            usleep(2*TURN_SLEEP);
        }
        else{
            //move forward
            switch (face){
                case 0:
                    car_x ++;
                    break;
                case 1:
                    car_y ++;
                    break;
                case 2:
                    car_x --;
                    break;
                case 3:
                    car_y --;
                    break;
            }
            map[car_x%50][car_y%50] = 0;
            car_forward();
            usleep(TURN_SLEEP);
        }
        printf("%d\n",sum);

        if(sum == 2500){
            car_stop();
            break;
        }
    }
}
int test_head(int next_head){
    printf("start test\r\n");

    int check = 2;
    while(check){
        zrcar_ultra_get_all(ult_data);
        printf("start ult_data: %f\r\n",ult_data[1]);
        if(ult_data[1] < DANGEROUS_DIS || ult_data[1] > MAX_DIS){
            xil_printf("start turn\r\n");
            check --;
            if(next_head > face){
                car_turn_l();
                usleep(MOVE_SLEEP);
                car_stop();
            }
            else{
                car_turn_r();
                usleep(MOVE_SLEEP);
                car_stop();
            }
            face = next_head;
        }
        else{
            switch(face){
                case 0:
                    car_x += 1;
                    break;
                case 1:
                    car_y += 1;
                    break;
                case 2:
                    car_x -= 1;
                    break;
                case 3:
                    car_y -= 1;
                    break;
            }
            car_forward();
            usleep(TURN_SLEEP);
            car_stop();
            break;
        }
    }
    return 0;
}
int find_path(int total_x,int total_y){

    int head = 0;
    int next_head = 0;
    int turn = 0;
    int x = total_x - car_x;
    int y = total_y - car_y;
    if(!(x || y)){
        return 1;
    }
    int head_x = (x < 0) ? 2 : 0;
    int head_y = (y < 0) ? 3 : 1;

    if( x*x - y*y < 0){
        head = head_y;
        next_head = head_x;
    }
    else{
        head = head_x;
        next_head = head_y;
    }
    turn = head - face;
    switch (turn){
        case 0:
            break;
        case 1:
            car_turn_l();
            usleep(MOVE_SLEEP);
            car_stop();
            break;
        case 2:
            car_turn_l();
            usleep(2*MOVE_SLEEP);
            car_stop();
            break;
        case 3:
            car_turn_l();
            usleep(3*MOVE_SLEEP);
            car_stop();
            break;
        case -1:
            car_turn_r();
            usleep(MOVE_SLEEP);
            car_stop();
            break;
        case -2:
            car_turn_r();
            usleep(2*MOVE_SLEEP);
            car_stop();
            break;
        case -3:
            car_turn_r();
            usleep(3*MOVE_SLEEP);
            car_stop();
            break;
    }
    face = head;
    test_head(next_head);
    return 0;
}
void goto_point(int total_x,int total_y){
    printf("x:%d, y:%d\r\n",total_x,total_y);
    while(!find_path(total_x,total_y)){
            xil_printf("location: ( %d, %d)\r\n", car_x,car_y);
    }
    printf("location: %d, %d\r\n",car_x, car_y);
}
int main()
{
    /*********init*******/
    zrcar_wheel_init();
    zrcar_ultra_init();
    zrcar_bluetooth_init();
    u8 input = 0;
    u8 old_input = 0;
    int old_map[50][50];
    int map[50][50];
    int place_index = 0;
    int check = 0;

    while(1)
    {
        //while(flag == 0)  //receive until finished receiving a frame.
        check = zrcar_bluetooth_recv(&input,1);
        place_index = input - 'a';
        //printf("x:%d, y:%d\r\n",x[place_index],y[place_index]);
        if(old_input != input){
            if(input == '1'){
                make_map(map,old_map);
            }
            else if(input == '#'){
                break;
            }
            else if(place_index >= 0 && place_index < 10){
                goto_point(x[place_index],y[place_index]);
            }
            old_input = input;
        }
        usleep(50000);
    }

    return 0;
}
